#pragma once

#include "behaviortree_cpp/behavior_tree.h"
#include "behaviortree_cpp/json_export.h"

namespace fishbot
{
  namespace behaviors
  {

    struct Vector3D
    {
      double x;
      double y;
      double z;
    };

    struct Vector4D
    {
      double x;
      double y;
      double z;
      double w;
    };

    inline void ToJson(nlohmann::json &dest, const Vector3D &pose)
    {
      dest["x"] = pose.x;
      dest["y"] = pose.y;
      dest["z"] = pose.z;
    }

    inline void ToJson(nlohmann::json &dest, const Vector4D &pose)
    {
      dest["x"] = pose.x;
      dest["y"] = pose.y;
      dest["z"] = pose.z;
      dest["w"] = pose.w;
    }
  }
}

namespace BT
{

  template <>
  inline fishbot::behaviors::Vector3D convertFromString(StringView key)
  {
    const auto parts = BT::splitString(key, ',');
    if (parts.size() != 3)
    {
      throw BT::RuntimeError("invalid input)");
    }

    fishbot::behaviors::Vector3D output;
    output.x = convertFromString<double>(parts[0]);
    output.y = convertFromString<double>(parts[1]);
    output.z = convertFromString<double>(parts[2]);
    return output;
  }

  template <>
  inline fishbot::behaviors::Vector4D convertFromString(StringView key)
  {
    const auto parts = BT::splitString(key, ',');
    if (parts.size() != 4)
    {
      throw BT::RuntimeError("invalid input)");
    }

    fishbot::behaviors::Vector4D output;
    output.x = convertFromString<double>(parts[0]);
    output.y = convertFromString<double>(parts[1]);
    output.z = convertFromString<double>(parts[2]);
    output.w = convertFromString<double>(parts[3]);
    return output;
  }

  template <>
  inline std::vector<std::string> convertFromString(StringView key)
  {
    const auto parts = BT::splitString(key, ',');
    if(parts.size() <= 0)
    {
      throw BT::RuntimeError("invalid input)");
    }

    std::vector<std::string> output;
    for(const auto& str : parts)
    {
      output.push_back(std::string(str));

    }
    
    return output;
    
  }
} // namespace BT